﻿using System;
using System.Collections;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Windows.Forms;
using System.Diagnostics;
using System.Threading;
using System.IO;
using MotionPlanning.MotionPlanningComponents;
using MotionPlanning.MotionPlanningPrimitives;
using MotionPlanning.MotionPlanningAlgorithms;
using MotionPlanning.MotionPlanningAlgorithms.RRTstar;
using MotionPlanning.MotionPlanningAlgorithms.RRT_Standard;
using MotionPlanning.MotionPlanningAlgorithms.ioRRT_Prototype;
using MotionPlanning.MotionPlanningAlgorithms.RRTas_Standard;
using MotionPlanning.MotionPlanningComponents.MotionPlanningSystems;
using MotionPlanning.MotionPlanningComponents.Visualizations;
using MotionPlanning.MotionPlanningExperiments;

namespace MotionPlanning
{
    public partial class Form1 : Form, UIForm
    {
        public Form1()
        {
            InitializeComponent();
            this.FormClosing += new FormClosingEventHandler(Form1_FormClosing);
        }

        void Form1_FormClosing(object sender, FormClosingEventArgs e)
        {
            if(currentExperiment != null)
                currentExperiment.Purge();
        }
        
        private Experiment currentExperiment;

        void CanvasRegion_Paint(object sender, PaintEventArgs e)
        {
            currentExperiment.FlushCanvas_primary();
        }

        private void button43_Click(object sender, EventArgs e)
        {
            currentExperiment.Start();
        }

        public Button get_button_start()
        {
            return button_start;
        }

        public RichTextBox get_console_box()
        {
            return richTextBox1;
        }

        public RichTextBox get_monitor_box()
        {
            return richTextBox2;
        }

        public Graphics get_canvas_primary()
        {
            return CanvasRegion.CreateGraphics();
        }

        private void button43_Click_1(object sender, EventArgs e)
        {
            currentExperiment.FlushCanvas_primary();
        }

        private void button2_Click(object sender, EventArgs e)
        {
            RoboticSystemComponent system = new System_Point2D();

            Visualizer_Point2D visual = new Visualizer_Point2D(
                CanvasRegion.CreateGraphics(),
                new Rectangle(0, 0, CanvasRegion.Width, CanvasRegion.Height));
            Planner_RRTBased planner = new ioRRT(system, visual, "../../MotionPlanningResources/Point2D_ioRRT.kdl");
            currentExperiment = new Experiment_Multithreading("Point2D_ioRRT", 6, planner, this, 1500000);

            CanvasRegion.Paint += new PaintEventHandler(CanvasRegion_Paint);
        }

        private void button3_Click(object sender, EventArgs e)
        {
            RoboticSystemComponent system = new System_Car1st();

            Visualizer_Car1st visual = new Visualizer_Car1st(
                CanvasRegion.CreateGraphics(),
                new Rectangle(0, 0, CanvasRegion.Width, CanvasRegion.Height));
            visual.Set_ActiveArea_Primary(CanvasRegion);
            //Planner_RRTBased planner = new ioRRT(system, visual, "c:/tmp/MotionPlanningResources/Car1st_test.kdl");
            Planner_RRTBased planner = new ioRRT(system, visual, "../../MotionPlanningResources/Car1st_ioRRT.kdl");
            currentExperiment = new Experiment_Multithreading("Car1st_ioRRT", 6, planner, this, 1500000);

            CanvasRegion.Paint += new PaintEventHandler(CanvasRegion_Paint);
        }

        private void button4_Click(object sender, EventArgs e)
        {
            System_Impreza system = new System_Impreza();

            Visualizer_Impreza visual = new Visualizer_Impreza(
                CanvasRegion.CreateGraphics(),
                new Rectangle(0, 0, CanvasRegion.Width, CanvasRegion.Height));
            visual.Set_ActiveArea_Primary(richTextBox2);
            Planner_RRTBased planner = new ioRRT(system, visual, "../../MotionPlanningResources/Impreza_ioRRT.kdl");
            ((ioRRT)planner).SetGoalCheck(true);
            currentExperiment = new Experiment_Multithreading("Impreza_ioRRT", 6, planner, this, 3000000);

            CanvasRegion.Paint += new PaintEventHandler(CanvasRegion_Paint);
        }

        private void button1_Click(object sender, EventArgs e)
        {
            StreamWriter file = new StreamWriter("solution.kdl");
            Trajectory sol = currentExperiment.get_CurrentPlanner().GetSolution();
            List<Edge> edges = sol.GetEdges();
            file.Write("<trajectory>\n");
            for (int i = 0; i < edges.Count - 1; ++i)
            {
                StringBuilder sb = new StringBuilder();
                sb.Append("\t<edge>\n");
                    sb.Append("\t\t<q>");
                        sb.Append(MotionPlanningUtils.ArrayToString( edges[i].get_entry().get_Value().get_q() ));
                    sb.Append("</q>\n");
                    sb.Append("\t\t<ctrl>");
                        sb.Append(MotionPlanningUtils.ArrayToString(edges[i].get_Value().get_ctrl_inc()));
                    sb.Append("</ctrl>\n");
                sb.Append("\t</edge>\n");
                file.Write(sb.ToString());
            }
            file.WriteLine("</trajectory>\n");
            file.Close();
        }

    }


}
